
#include "mpu9250.h"
#include <string.h>
#include "cmsis_os.h"
#define mpu_select() HAL_GPIO_WritePin(MPU9250_CS_GPIO, MPU9250_CS_PIN, GPIO_PIN_RESET)
#define mpu_deselect() HAL_GPIO_WritePin(MPU9250_CS_GPIO, MPU9250_CS_PIN, GPIO_PIN_SET)

#define DATABUF_SIZ 16

MPU9250 mpu9250;
MPU9250_Zero_TypeDef zero_bais;
/*** basic mpu9250 operate ***/
/*
 * @brief   write mpu9250 reg through spi
 * @param   address: address of reg to write
 * @param   byte: byte to write
 * */
static void mpu_w_reg(uint8_t address, uint8_t byte)
{
	// spi_w_bytes(&MPU9250_SPI, address, &byte, 1);

	uint8_t send_buff[2] = {address, byte};

	mpu_select();
	HAL_SPI_Transmit(&MPU9250_SPI,send_buff,2,0x01f4);
	osDelay(1);
	mpu_deselect();
}

/*
 * @brief   read mpu9250 regs through spi
 * @param   address: address of reg to write
 * @param   num: number of byte to read
 * @return  read bytes array
 * */
static void mpu_r_regs(uint8_t address, uint8_t num,uint8_t *data)
{

	//spi_r_bytes(&MPU9250_SPI, address, num);

	mpu_select();

	uint8_t transfar_buff[1]={address | 0x80};
	uint8_t receive_buff[DATABUF_SIZ+1]={0};
	HAL_SPI_TransmitReceive(&MPU9250_SPI,transfar_buff,receive_buff,1+num,0xff);//由于spi的协议，发一个字节就得接收一个字节 所以这里需要多读一个字节
	memcpy(data,receive_buff+1,num); // 从 receive_buff 的第二个字节开始复制 num 个字节到 dataBuf
	mpu_deselect();

}


/******* basic ak8963 operate *******/
/*
 * @brief   write AK8963 regs through I2C in MPU9250
 * @param   address: address of AK8963 reg to write
 * @param   byte: byte to write
 * */
static void mpu_w_ak8963_reg(uint8_t address, uint8_t byte)
{
	mpu_w_reg(I2C_SLV0_ADDR, AK8963_I2C_ADDR);//设置I2C的从机地址 0x0c:AK8963的默认IIC地址
	mpu_w_reg(I2C_SLV0_REG, address); 	//设置从机0寄存器地址x [读写AK8963的寄存器地址]
	mpu_w_reg(I2C_SLV0_DO, byte);		//写入从机0寄存器x的数据 [写入数据]
	mpu_w_reg(I2C_SLV0_CTRL, 0x81);//i2c从机0控制 bit7:SLV0_EN  还有i2c的长度为1
}
/*
 * @brief read AK8963 regs through I2C in MPU9250
 * @param   address: first address of AK8963 regs to read
 * @param   num: number of byte to read
 * @return  read bytes array
 * */
static void mpu_r_ak8963_regs(uint8_t address, uint8_t num,uint8_t *data)
{
	mpu_w_reg(I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
	mpu_w_reg(I2C_SLV0_REG, address);
	mpu_w_reg(I2C_SLV0_CTRL, 0x80 | num);
	HAL_Delay(1);
	mpu_r_regs(EXT_SENS_DATA_00, num,data);
}
/*
 * @brief   read ak8963 WHO_AM_I reg
 * @return  AK8963 WHO_AM_I value, expected to be 0x48
 * */
uint8_t mpu_r_ak8963_WhoAmI(MPU9250 *mpu)
{
	uint8_t dataBuf[1];
	mpu_r_ak8963_regs(AK8963_WHOAMI_REG, 1,dataBuf);
	return dataBuf[0];
}
/*
 * @brief   read mpu9250(mpu6500) WHO_AM_I reg
 * @return  mpu9250(mpu6500) WHO_AM_I value, expected to be 0x48
 * */
uint8_t mpu_r_WhoAmI(MPU9250 *mpu)
{
	uint8_t dataBuf[1];
	mpu_r_regs(WHO_AM_I, 1,dataBuf);
	return dataBuf[0];
}
/*
 * @brief   init origin data
 * */
static void MPU9250_StructInit(MPU9250 *mpu)
{

	for (uint8_t i = 0; i < 3; i++)
	{
		mpu->mpu_data.Accel[i] = 0;
		mpu->mpu_data.Gyro[i] = 0;
		mpu->mpu_data.Magn[i] = 0;

		mpu->mpu_prop.Accel_row[i] = 0;
		mpu->mpu_prop.Gyro_row[i] = 0;
		mpu->mpu_prop.Magn_row[i] = 0.0;

		mpu->mpu_zero.acc_bias[i]=0;
		mpu->mpu_zero.gyro_bias[i]=0;
	}
}
/*
 * @brief   init mpu9250
 * */
uint8_t MPU9250_Init(MPU9250 *mpu)
{
	MPU9250_StructInit(mpu);

	mpu_w_reg(PWR_MGMT_1, (uint8_t) 0x80); // H_RESET bit to reset mpu9250 复位9250
	HAL_Delay(10);

	mpu_w_reg(USER_CTRL, (uint8_t) 0x20); // enable I2C master mode 开启i2c	master模式 准备接下来的设置
	mpu_w_reg(I2C_MST_CTRL, (uint8_t) 0x0D); // set I2C clock speed to 400kHz 设置iic 的时钟频率为400Khz
	mpu_w_ak8963_reg(AK8963_CNTL1_REG, (uint8_t) 0x00); // set AK8963 to power down
	mpu_w_reg(PWR_MGMT_1, (uint8_t) 0x80); // reset MPU9250, Bit[7] will auto clear
	HAL_Delay(10);

	mpu_w_ak8963_reg(AK8963_CNTL2_REG, AK8963_CNTL2_SRST); // reset AK8963
	mpu_w_reg(PWR_MGMT_1, (uint8_t) 0x01); // select clock source
	mpu_w_reg(PWR_MGMT_2, (uint8_t) 0x00); // enable accel and gyro

	/* init GYRO and ACCEL */
	mpu_w_reg(SMPLRT_DIV, (uint8_t) 0x00); // SAMPLE_RATE= Internal_Sample_Rate / (1 + SMPLRT_DIV), Internal_Sample_Rate==8K
	mpu_w_reg(GYRO_CONFIG, (uint8_t) MPU9250_Gyro_Range_2000dps); // gyro full scale select
	mpu_w_reg(ACCEL_CONFIG, (uint8_t) MPU9250_Accel_Range_16G); // accel full scale select
	mpu_w_reg(ACCEL_CONFIG_2, (uint8_t) MPU9250_Accel_DLPFBandwidth_460);
	mpu_w_reg(CONFIG, (uint8_t) MPU9250_Gyro_DLPFBandwidth_250);
	// /* init MAG */
	// mpu_w_reg(USER_CTRL, (uint8_t) 0x10); // disable I2C master mode
	// mpu_w_reg(I2C_MST_CTRL, (uint8_t) 0x0D); // set I2C clock speed to 400kHz, reg36
	// mpu_w_ak8963_reg(AK8963_CNTL1_REG, (uint8_t) 0x00); // set AK8963 to power down
	// HAL_Delay(100);
	// mpu_w_ak8963_reg(AK8963_CNTL1_REG, (uint8_t) 0x0f); // set AK8963 to Fuse ROM access mode
	// HAL_Delay(100);
	// mpu_w_ak8963_reg(AK8963_CNTL1_REG, (uint8_t) 0x00); // set AK8963 to power down
	// HAL_Delay(100);
	// mpu_w_ak8963_reg(AK8963_CNTL1_REG, (uint8_t) 0x16); // AK8963 working on Continuous measurement mode 2 & 16-bit output
	// HAL_Delay(100);
	// mpu_w_reg(PWR_MGMT_1, (uint8_t) 0x01); // select clock source
	// mpu_r_ak8963_regs(MAG_XOUT_L, 7,NULL);

	return 0x00;
}
/*
 * @brief   read accel origin value and calculate real value
 *          data will be stored in mpu
 * */
void MPU9250_ReadAccel(MPU9250 *mpu)
{

	static uint8_t dataBuf[DATABUF_SIZ] = {0}; // all read data will store in this array
	// m/s
	mpu_r_regs(ACCEL_XOUT_H, 6,dataBuf);

	// calculate x axis
	mpu->mpu_prop.Accel_row[0] = (((int16_t)dataBuf[0] << 8) | dataBuf[1]);
	mpu->mpu_data.Accel[0] = ((float)mpu->mpu_prop.Accel_row[0] +2637.272)/ 208.980;

	// calculate y axis
	mpu->mpu_prop.Accel_row[1] = (((int16_t)dataBuf[2] << 8) | dataBuf[3]);
	mpu->mpu_data.Accel[1] = ((float)mpu->mpu_prop.Accel_row[1]+507.3805)/ 208.980;

	// calculate z axis
	mpu->mpu_prop.Accel_row[2] = (((int16_t)dataBuf[4] << 8) | dataBuf[5]);
	mpu->mpu_data.Accel[2] = ((float)mpu->mpu_prop.Accel_row[2]+412.842) / 208.980;
}
/*
 * @brief   read gyro origin value and calculate real value
 *          data will be stored in mpu
 * */
void MPU9250_ReadGyro(MPU9250 *mpu)
{
	
	static uint8_t dataBuf[DATABUF_SIZ] = {0}; // all read data will store in this array
	// d/s
	mpu_r_regs(GYRO_XOUT_H, 6,dataBuf);
	// calculate x axis
	mpu->mpu_prop.Gyro_row[0] = ((int16_t)dataBuf[0] << 8) | dataBuf[1];
	mpu->mpu_data.Gyro[0] = ((float)mpu->mpu_prop.Gyro_row[0]+20.7825)*0.001064;

	// calculate y axis
	mpu->mpu_prop.Gyro_row[1] = ((int16_t)dataBuf[2] << 8) | dataBuf[3];
	mpu->mpu_data.Gyro[1] = ((float)mpu->mpu_prop.Gyro_row[1]+6.032 )*0.001064;

	// calculate z axis
	mpu->mpu_prop.Gyro_row[2] = ((int16_t)dataBuf[4] << 8) | dataBuf[5];
	mpu->mpu_data.Gyro[2] = ((float)mpu->mpu_prop.Gyro_row[2]-4.1) *0.001064;
}
/**
 * @brief   read temperature value and calculate real value
 *          data will be stored in mpu
*/
void MPU9250_ReadTemp(MPU9250 *mpu)
{
	static uint8_t dataBuf[DATABUF_SIZ] = {0}; // all read data will store in this array
	mpu_r_regs(TEMP_OUT_H, 2,dataBuf);
	mpu->mpu_prop.Temp_row = ((int16_t)dataBuf[0] << 8) | dataBuf[1];
	mpu->mpu_data.Temp = ((float)mpu->mpu_prop.Temp_row)/333.87 + 21.0; // temp in Celsius

}

void MPU9250_Read_ALL(MPU9250 *mpu)
{
	static uint8_t dataBuf[DATABUF_SIZ] = {0}; // all read data will store in this array

	mpu_r_regs(ACCEL_XOUT_H,14,dataBuf);

	// calculate x axis
	mpu->mpu_prop.Accel_row[0] = ((int16_t)dataBuf[0] << 8) | dataBuf[1];
	mpu->mpu_data.Accel[0] = ((float)mpu->mpu_prop.Accel_row[0] -(zero_bais.acc_bias[0])) / 208.980;

	// calculate y axis
	mpu->mpu_prop.Accel_row[1] = ((int16_t)dataBuf[2] << 8) | dataBuf[3];
	mpu->mpu_data.Accel[1] = ((float)mpu->mpu_prop.Accel_row[1]-(zero_bais.acc_bias[1])) / 208.980;

	// calculate z axis
	mpu->mpu_prop.Accel_row[2] = ((int16_t)dataBuf[4] << 8) | dataBuf[5];
	mpu->mpu_data.Accel[2] = (((float)mpu->mpu_prop.Accel_row[2]-(zero_bais.acc_bias[2])) / 208.980)+9.81;

	// calculate temperature
	mpu->mpu_prop.Temp_row = ((int16_t)dataBuf[6] << 8) | dataBuf[7];
	mpu->mpu_data.Temp = ((float)mpu->mpu_prop.Temp_row)/333.87 + 21.0; // temp in Celsius

	// calculate x axis
	mpu->mpu_prop.Gyro_row[0] = ((int16_t)dataBuf[8] << 8) | dataBuf[9];
	mpu->mpu_data.Gyro[0] = ((float)mpu->mpu_prop.Gyro_row[0]-(zero_bais.gyro_bias[0]))*0.001064;

	// calculate y axis
	mpu->mpu_prop.Gyro_row[1] = ((int16_t)dataBuf[10] << 8) | dataBuf[11];
	mpu->mpu_data.Gyro[1] = ((float)mpu->mpu_prop.Gyro_row[1]-(zero_bais.gyro_bias[1] ))*0.001064;

	// calculate z axis
	mpu->mpu_prop.Gyro_row[2] = ((int16_t)dataBuf[12] << 8) | dataBuf[13];
	mpu->mpu_data.Gyro[2] = ((float)mpu->mpu_prop.Gyro_row[2]-(zero_bais.gyro_bias[2])) *0.001064;
}
/*
 * @brief   read mag origin value and calculate real value
 *          data will be stored in mpu
 * */
void MPU9250_ReadMag(MPU9250 *mpu)
{
	static uint8_t dataBuf[DATABUF_SIZ] = {0}; // all read data will store in this array
	uint8_t mag_adjust[3] = {0};
	uint8_t mag_buffer[6] = {0};

	mpu_r_ak8963_regs(AK8963_ASAX, 3,dataBuf);
	mag_adjust[0] = dataBuf[0];
	mag_adjust[1] = dataBuf[1];
	mag_adjust[2] = dataBuf[2];

	// read AK8963_ST2_REG is necessary
	// ST2 register has a role as data reading end register(on page 51)

	mpu_r_ak8963_regs(MAG_XOUT_L, 1,dataBuf);
	mag_buffer[0] = dataBuf[0];

	mpu_r_ak8963_regs(AK8963_ST2_REG, 1,dataBuf); // data read finish reg
	mpu_r_ak8963_regs(MAG_XOUT_H, 1,dataBuf);
	mag_buffer[1] = dataBuf[0];

	mpu_r_ak8963_regs(AK8963_ST2_REG, 1,dataBuf);
	mpu_r_ak8963_regs(MAG_YOUT_L, 1,dataBuf);
	mag_buffer[2] = dataBuf[0];

	mpu_r_ak8963_regs(AK8963_ST2_REG, 1,dataBuf);
	mpu_r_ak8963_regs(MAG_YOUT_H, 1,dataBuf);
	mag_buffer[3] = dataBuf[0];

	mpu_r_ak8963_regs(AK8963_ST2_REG, 1,dataBuf);
	mpu_r_ak8963_regs(MAG_ZOUT_L, 1,dataBuf);
	mag_buffer[4] = dataBuf[0];

	mpu_r_ak8963_regs(AK8963_ST2_REG, 1,dataBuf);
	mpu_r_ak8963_regs(MAG_ZOUT_H, 1,dataBuf);
	mag_buffer[5] = dataBuf[0];

	mpu_r_ak8963_regs(AK8963_ST2_REG, 1,dataBuf);

	mpu->mpu_prop.Magn_row[0] = ((int16_t)mag_buffer[1] << 8) | mag_buffer[0];
	mpu->mpu_prop.Magn_row[1] = ((int16_t)mag_buffer[3] << 8) | mag_buffer[2];
	mpu->mpu_prop.Magn_row[2] = ((int16_t)mag_buffer[5] << 8) | mag_buffer[4];

	// calculate real value, check page53
	mpu->mpu_data.Magn[0] = (float)mpu->mpu_prop.Magn_row[0] * (((mag_adjust[0] - 128) / 256.0) + 1);
	mpu->mpu_data.Magn[0] = mpu->mpu_prop.Magn_row[0] * 0.15;

	mpu->mpu_data.Magn[1] = (float)mpu->mpu_prop.Magn_row[1] * (((mag_adjust[1] - 128) / 256.0) + 1);
	mpu->mpu_data.Magn[1] = mpu->mpu_prop.Magn_row[1] * 0.15;

	mpu->mpu_data.Magn[2] = (float)mpu->mpu_prop.Magn_row[2] * (((mag_adjust[2] - 128) / 256.0) + 1);
	mpu->mpu_data.Magn[2] = mpu->mpu_prop.Magn_row[2] * 0.15;
}
/*
 * @brief   read all 9 DOF data
 *          data will be stored in mpu
 * */
void MPU9250_ReadData(MPU9250 *mpu)
{
	MPU9250_ReadAccel(mpu);
	MPU9250_ReadGyro(mpu);
	MPU9250_ReadMag(mpu);
}
